 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Creating a Diagnostic Analyzer
Description: An example of how to write your own diagnostic analyzer plug-in for the diagnostic_aggregator.Tutorial Level: ADVANCED
Contents
Overview
Diagnostic Analyzers are used by the diagnostic_aggregator. They can collect, analyze and process raw diagnostics data. Robot developers should create an analyzer for their robot if they wish to examine diagnostics data across the entire robot system.
Basic diagnostic analyzers are included in the diagnostic_aggregator package. For most purposes, the diagnostic_aggregator::GenericAnalyzer and diagnostic_aggregator::AnalyzerGroup are sufficient for diagnostics analysis. See the Using the GenericAnalyzer tutorial for details on both basic analyzers.
By creating customized diagnostic analyzers, robot operators can extend the diagnostics toolchain across an entire robot. Without the diagnostic_aggregator, diagnostic output and processing could only be done at the node or process level. But for the full robot, the interaction between the different systems becomes important.
In this tutorial, we'll write a diagnostic analyzer to analyze the motors (or EtheCAT devices) of a PR2. When the power board runstop is hit, we'll disregard error messages from the motors and report 'OK' status for the motor driver. (By default, when the runstop is hit, the motors report an error because they are in undervoltage).
Our analyzer will be loaded by the aggregator_node as a plugin, and will give it some diagnostics input to verify that it works.
The analyzer in this tutorial would need some fleshing out to be used on a real robot. It would need to validate input, check for other problems with the motors, etc. For clarity, that code has been removed for this example. Extending and hardening this analyzer is an exercise for the reader.
Making the Package and File
First, we need to make a package for this new analyzer. In the sandbox folder, type:
catkin_create_pkg pr2_motors_analyzer diagnostic_aggregator pluginlib roscpp diagnostic_msgs
cd into the package directory and create a new file include/pr2_motors_analyzer/pr2_motors_analyzer.h containing:
1 #ifndef PR2_MOTORS_ANALYZER_H
2 #define PR2_MOTORS_ANALYZER_H
3
4 #include <ros/ros.h>
5 #include <diagnostic_aggregator/analyzer.h>
6 #include <diagnostic_aggregator/status_item.h>
7 #include <diagnostic_msgs/DiagnosticStatus.h>
8 #include <pluginlib/class_list_macros.h>
9 #include <string>
10
11 namespace diagnostic_aggregator {
12
13 class PR2MotorsAnalyzer : public Analyzer
14 {
15 public:
16 PR2MotorsAnalyzer();
17
18 ~PR2MotorsAnalyzer();
19
20 bool init(const std::string base_name, const ros::NodeHandle &n);
21
22 bool match(const std::string name);
23
24 bool analyze(const boost::shared_ptr<StatusItem> item);
25
26 std::vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > report();
27
28 std::string getPath() const { return path_; }
29
30 std::string getName() const { return nice_name_; }
31
32 private:
33
34 // Store status item for EtherCAT master
35 boost::shared_ptr<StatusItem> eth_master_item_;
36
37 std::string path_, nice_name_, power_board_name_;
38
39 bool runstop_hit_, has_initialized_, has_power_data_, has_eth_data_;
40 };
41
42 }
43 #endif //PR2_MOTORS_ANALYZER_H
44
This defines the header for our diagnostic analyzer. We're defining all the pure virtual functions from the diagnostic_aggregator::Analyzer base class, i.e.
init
match
analyze
report
getPath
getName
To make this analyzer more useful, we may want to track EtherCAT hubs and other EtherCAT devices. Right now, we'll just look at the EtherCAT Master.
Defining Analyzer Functions
Now we're going to define the functions we spelled out in our header. Make directory "src" in the pr2_motors_analyzer package. Open the file "src/pr2_motors_analyzer.cpp".
1 #include "pr2_motors_analyzer/pr2_motors_analyzer.h"
2
3 using namespace diagnostic_aggregator;
4 using namespace std;
5
6 PLUGINLIB_REGISTER_CLASS(PR2MotorsAnalyzer,
7 diagnostic_aggregator::PR2MotorsAnalyzer,
8 diagnostic_aggregator::Analyzer)
9
10 PR2MotorsAnalyzer::PR2MotorsAnalyzer() :
11 path_(""), nice_name_("Motors"), power_board_name_(""),
12 runstop_hit_(false), has_initialized_(false), has_power_data_(false),
13 has_eth_data_(false)
14 { }
15
16 PR2MotorsAnalyzer::~PR2MotorsAnalyzer() { }
The line with "PLUGINLIB_..." uses a pluginlib macro to allow this class to be loaded as a plugin. We'll talk more about using pluginlib more below.
Initialization
Fill out the init() function:
1 bool PR2MotorsAnalyzer::init(const string base_name, const ros::NodeHandle &n)
2 {
3 // path_ = BASE_NAME/Motors
4 path_ = base_name + "/" + nice_name_;
5
6 if (!n.getParam("power_board_name", power_board_name_))
7 {
8 ROS_ERROR("No power board name was specified in PR2MotorsAnalyzer! Power board must be \"Power board 10XX\". Namespace: %s", n.getNamespace().c_str());
9 return false;
10 }
11
12 // Make a "missing" item for the EtherCAT Master
13 boost::shared_ptr<StatusItem> item(new StatusItem("EtherCAT Master"));
14 eth_master_item_ = item;
15
16 has_initialized_ = true;
17
18 return true;
19 }
We're requiring one parameter for our analyzer: power_board_name. The power board name is the diagnostic status name of the power board, which is something like "Power board 10XX". We could choose to specify the serial number of the power board instead. Note that both of these values are unique to each robot.
As we can see, each of the Analyzers only looks at the item name to "match" an item.
A real analyzer would check that the power board name is valid. This is an exercise for the reader. We can't look for any diagnostic name that starts with "Power board", because in some cases, we may see more than one.
Matching and Analyzing
We report a match if we see a motor, or see our power board. The match function will only be called once for every new status item, since the aggregator_node caches the results.
Note: Since match() is only called once, analyzers cannot change their matching rule.
1 bool PR2MotorsAnalyzer::analyze(const boost::shared_ptr<StatusItem> item)
2 {
3 if (item->getName() == power_board_name_)
4 {
5 has_power_data_ = true;
6 runstop_hit_ = item->getValue("Runstop hit") == "True" || item->getValue("Estop hit") == "True";
7 return false; // Won't report this item
8 }
9
10 // We know our item is "EtherCAT Master"
11 eth_master_item_ = item;
12 has_eth_data_ = true;
13
14 return true;
15 }
In the analyze() function, we return true if we will report that item in our output. Since we're only interested in looking and the power board data, and not reporting it, we return false. We will report the "EtherCAT Master" item.
analyze() is only called for matching items. We could add an assert() to check that the item is either the power board name or "EtherCAT Master".
Reporting
Analyzers collect the raw diagnostics for every new message. At 1Hz, they must report the status.
1 vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > PR2MotorsAnalyzer::report()
2 {
3 boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> eth_stat = eth_master_item_->toStatusMsg(path_);
4
5 // If we have power data, and runstop is hit, we'll suppress errors
6 if (has_eth_data_ && has_power_data_ && runstop_hit_)
7 {
8 eth_stat->level = diagnostic_msgs::DiagnosticStatus::OK;
9 }
10
11 vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > output;
12 output.push_back(eth_stat);
13
14 return output;
15 }
Now we've successfully implemented our analyzer. Close out the "pr2_motors_analyzer.cpp" file, and save it.
Building and Testing the Analyzer
We'll show how to load the analyzer as a plugin and test it with some simulated diagnostics data.
To test this analyzer, we can write a simple Python node that publishes diagnostics just like a real power board and EtherCAT Master. We can view the raw and processed diagnostics data to verify our analyzer.
Building and Loading Plugin
In your new package, open the file "CMakeLists.txt". Add the following lines to build the analyzer as a library.
add_library(pr2_motors_analyzer src/pr2_motors_analyzer.cpp)
See CMakeLists for details on cmake.
Now make a new file "pr2_motors_analyzer_plugin.xml". See the documentation for pluginlib for details on this file. As you can see, we give our class name as the "name", and the fully qualified namespace of the analyzer as the "type".
1 <library path="lib/libpr2_motors_analyzer" >
2 <class name="PR2MotorsAnalyzer" type="diagnostic_aggregator::PR2MotorsAnalyzer" base_class_type="diagnostic_aggregator::Analyzer">
3 <description>
4 PR2MotorsAnalyzer is tutorial for writing diagnostic analyzers.
5 </description>
6 </class>
7 </library>
Add the following line to your "package.xml" file to export the plugin.
1 <export>
2 <!-- You may already have an "export" tag. If so, just add this inside it-->
3 <diagnostic_aggregator plugin="${prefix}/pr2_motors_analyzer_plugin.xml" />
4 </export>
Type "catkin_make pr2_motors_analyzer" to build the analyzer.
Loading Plugin
First, we'll check that our new plugin is visible to pluginlib and the diagnostic_aggregator. Use rospack to search for plugins.
$ rospack plugins --attrib=plugin diagnostic_aggregator
If you see "pr2_motors_analyzer PATH/TO/SANDBOX", then your plugin is registered. If not, double check the dependencies, then the manifest.xml.
To test that the analyzer loads, we'll need to create a YAML file that will have the parameters for the diagnostic_aggregator::PR2MotorsAnalyzer. Make a directory "test" and open a file called "test/pr2_motors_analyzer_load.yaml"
analyzers:
motors:
type: PR2MotorsAnalyzer
power_board_name: Power board 1000Make another file called "test/test_pr2_motors_analyzer_load.launch"
1 <launch>
2 <test pkg="diagnostic_aggregator" type="analyzer_loader"
3 name="loader" test-name="pr2_motors_analyzer_load_test" >
4 <rosparam command="load"
5 file="$(find pr2_motors_analyzer)/test/pr2_motors_analyzer_load.yaml" />
6 </test>
7 </launch>
Now use rostest to make sure the new plugin loads.
rostest test/test_pr2_motors_analyzer_load.launch
It should load successfully.
Add
rosbuild_add_rostest(test/test_pr2_motors_analyzer_load.launch)
to the "CMakeLists.txt" file. Type "make test" to run this load test. This will make sure your analyzer always loads successfully given correct parameters and prevent regressions.
Generating Diagnostics Input
We'll use a rospy publisher to generate fake diagnostics input and view the diagnostics in the rqt_runtime_monitor and rqt_robot_monitor (runtime_monitor, robot_monitor respectively if earlier than Groovy) simultaneously.
Make a directory "scripts" in the pr2_motors_analyzer package. Open "scripts/pr2_power_pub.py"
1 #!/usr/bin/env python
2
3 import roslib; roslib.load_manifest('pr2_motors_analyzer')
4
5 import rospy
6 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
7
8 if __name__ == '__main__':
9 rospy.init_node('pr2_motor_power_sim')
10
11 pub = rospy.Publisher('/diagnostics', DiagnosticArray)
12
13 array = DiagnosticArray()
14 # Fake power board status, estop is on
15 power_stat = DiagnosticStatus(name = 'Power board 1000', level = 0,
16 message = 'Running')
17 power_stat.values = [ KeyValue(key = 'Runstop hit', value = 'False'),
18 KeyValue(key = 'Estop hit', value = 'False')]
19 # Fake EtherCAT Master status, all OK
20 eth_stat = DiagnosticStatus(name='EtherCAT Master', level = 0,
21 message = 'OK')
22
23 array.status = [ power_stat, eth_stat ]
24
25 my_rate = rospy.Rate(1.0)
26 while not rospy.is_shutdown():
27 pub.publish(array)
28 my_rate.sleep()
Make another file, "scripts/pr2_power_pub_estop_hit.py" and change these lines above:
1 # Fake power board status, estop is hit
2 power_stat = DiagnosticStatus(name = 'Power board 1000', level = 0,
3 message = 'Running')
4 power_stat.values = [ KeyValue(key = 'Runstop hit', value = 'False'),
5 KeyValue(key = 'Estop hit', value = 'True')]
6 # Fake EtherCAT Master status, level is Error
7 eth_stat = DiagnosticStatus(name='EtherCAT Master', level = 2,
8 message = 'Motors Halted')
Make both files executable. Add dependencies for rospy and diagnostic_msgs in your "manifest.xml" file.
Testing Analyzer
Make a launch file "test/test_estop_ok.launch".
1 <launch>
2 <node pkg="pr2_motors_analyzer" type="pr2_power_pub.py" name="diag_pub" />
3
4 <node pkg="diagnostic_aggregator" type="aggregator_node" name="diag_agg" >
5 <rosparam command="load"
6 file="$(find pr2_motors_analyzer)/test/pr2_motors_analyzer_load.yaml" />
7 </node>
8 </launch>
This will publish fake data on the /diagnostics topic and launch the aggregator_node.
Launch this file, and run the rqt_runtime_monitor (a.k.a runtime_monitor) with:
$ rosrun rqt_runtime_monitor rqt_runtime_monitor $ rosrun runtime_monitor monitor ## Ealier than `Groovy`
The rqt_runtime_monitor will show the raw data on /diagnostics. You should see
EtherCAT Master: OK Power board 1000: Running
View the aggregated data with the rqt_robot_monitor (a.k.a robot_monitor):
$ rosrun rqt_robot_monitor rqt_robot_monitor $ rosrun robot_monitor robot_monitor ## Ealier than `Groovy`
Now test the analyzer when the estop is "hit". Shutdown the aggregator_node, runtime_monitor and rqt_robot_monitor. Copy the launch file above to "test/test_estop_hit.launch", and use the node "pr2_power_pub_estop_hit.py". Rerun everything as before.
In the rqt_runtime_monitor:
EtherCAT Master: Motors Halted # This is in "Error" state Power board 1000: Running
But the robot_monitor shows the "Motors" with a green check, which means they're OK.
The PR2MotorsAnalyzer has successfully cleared the error state of the motors using data from the power board.
